import ROSLIB from "roslib";
import nipplejs from "nipplejs";
import { cmd_vel_listener } from "./topic";

// 手柄管理器
var manager = null;
// 保存周期发射器的ID
var timer = null;
// 最大线速度
var ml = 0.3; // m/s
// 最大角速度
var ma = 0.4; // rad/s

/**
 * 创建移动主题，并由cmd_vel_listener节点发布该主题
 * @param {number} linear 
 * @param {number} angular 
 */
function move(linear, angular) {
  if (linear == 0 && angular == 0) {
    // console.log("stop");
  }
  var twist = new ROSLIB.Message({
    linear: {
      x: linear,
      y: 0,
      z: 0
    },
    angular: {
      x: 0,
      y: 0,
      z: angular
    }
  });
  // console.log(linear);
  cmd_vel_listener.publish(twist);
}

/**
 * 创建非等距手柄（手柄拉伸程度影响发送的速度参数）
 * @param {object} element 挂载手柄的DOM元素id
 */
function createjoystick(element = "stick") {
  if (!manager) {
    var linear_speed = 0;
    var angular_speed = 0;

    manager = nipplejs.create({
      zone: document.getElementById(element),
      threshold: 0.1,
      position: {
        top: 50 + "%",
        left: 90 + "%"
      },
      mode: "static",
      size: 150,
      color: "#300000",
      dynamicPage: true
    });

    manager.on("start", function () {
      console.log("Movement start");
      // 周期检测手柄位置并发送
      timer = setInterval(function () {
        // console.log("setInterval");
        move(linear_speed, angular_speed);
        console.log(linear_speed + " " + angular_speed);
      }, 75);
    });

    manager.on("move", function (event, nipple) {
      console.log("Moving");
      var max_linear = 0.3; // m/s
      var max_angular = 0.4; // rad/s
      var max_distance = 75.0; // pixels;
      linear_speed =
        (Math.sin(nipple.angle.radian) * max_linear * nipple.distance) /
        max_distance;
      angular_speed =
        (-Math.cos(nipple.angle.radian) * max_angular * nipple.distance) /
        max_distance;
      //   linear_speed =
      //   (Math.sin(nipple.angle.radian) * max_linear * 0.3) /
      //   max_distance;
      // angular_speed =
      //   (-Math.cos(nipple.angle.radian) * max_angular * 0.3) /
      //   max_distance;

      console.log(nipple.distance);
      console.log(linear_speed);
      console.log(angular_speed);
    });

    manager.on("end", function () {
      console.log("Movement end");
      if (timer) {
        clearInterval(timer);
        console.log("clearInterval");
      }
      move(0, 0);
    });
  }
}

// 等距手柄实际线速度
var ls = 0;
// 等距手柄实际角速度
var as = 0;
// 等距手柄固定长度
const distance = 50;

/**
 * 创建等距手柄（手柄拉伸程度不影响发送的速度参数，且恒为最大值）
 * @param {object} element 挂载手柄的DOM元素id
 */
function createJoyStickWithoutDistance(element = "stick") {
  if (!manager) {
    manager = nipplejs.create({
      zone: document.getElementById(element),
      threshold: 0.1,
      position: {
        // top: 50 + "%",
        // left: 50 + "%"
      },
      mode: "static",
      size: 150,
      color: "#300000",
      dynamicPage: true
    });

    // 周期检测手柄位置并发送
    timer = setInterval(function () {
      // console.log("setInterval");
      // console.log(ls + " " + as);
      // console.log("LS " + ls + " AS " + as);
      move(ls, as);
    }, 100);

    manager.on("start", function () {
      // console.log("Movement start");
      // 周期检测手柄位置并发送
      // timer = setInterval(function () {
      //   // console.log("setInterval");
      //   // console.log(ls + " " + as);
      //   move(ls, as);
      // }, 100);
    });

    manager.on("move", function (event, nipple) {
      // console.log("Moving");
      var max_distance = 75.0; // pixels;
      ls = (Math.sin(nipple.angle.radian) * ml * distance) / max_distance;
      as = (-Math.cos(nipple.angle.radian) * ma * distance) / max_distance;

      // console.log(nipple.distance);
      // console.log("ML " + ml + " MA " + ma);
    });

    manager.on("end", function () {
      // console.log("Movement end");
      // if (timer) {
      //   clearInterval(timer);
      //   console.log("clearInterval");
      // }
      ls = 0;
      as = 0;
      move(0, 0);
    });
  }
}

/**
 * 设置手柄最大速度
 * @param {number} newValue 
 */
function setStickMaxLinear(newValue) {
  ml = newValue;
  console.log("setStickMaxLinear:" + newValue);
}

/**
 * 设置手柄最大角速度
 * @param {number} newValue 
 */
function setStickMaxAngular(newValue) {
  ma = newValue;
  console.log("setStickMaxAngular:" + newValue);
}

/**
 * 销毁手柄，置空全局变量
 */
function destroyjoystick() {
  if (timer) {
    clearInterval(timer);
    timer = null;
  }

  if (manager) {
    manager.destroy();
    manager = null;
  }
}

/**
 * 销毁并重建手柄，相当于刷新
 */
function reCreateStick() {
  destroyjoystick();
  createjoystick();
}

export { reCreateStick, createjoystick, destroyjoystick, createJoyStickWithoutDistance, manager, setStickMaxLinear, setStickMaxAngular, ls, as, move };
